This commit add the last missing piece of code enabling dynamic
frequency scaling support for Armada 38x.

The main difference with Armada XP is that the Cortex A9 CPU
frequencies of the Armada 38x SoCs are not independent. Even if a SoC
contains a single CPU, some specific initialization has to be done at
pmsu level: this unit must not wait for the second CPU when the
frequency is modified.

Signed-off-by: Gregory CLEMENT <gregory.clement at free-electrons.com>

Original patch adapted by Turris for kernel 4.4 has been adapted
for kernel 4.9:
* use include/linux/mvebu-pmsu.h to pass variables and functions
  between pmsu.c and cpufreq.c
* cpufreq-dt does not have parameters any more, so revert to
  simple registration
Signed-off-by: Hannu Nyman <hannu.nyman@iki.fi>

--- a/arch/arm/mach-mvebu/pmsu.c
+++ b/arch/arm/mach-mvebu/pmsu.c
@@ -351,6 +351,13 @@ void mvebu_v7_pmsu_idle_exit(void)
 	/* cancel ask HW to power down the L2 Cache if possible */
 	reg = readl(pmsu_mp_base + PMSU_CTL_CFG(hw_cpu));
 	reg &= ~PMSU_CTL_CFG_L2_PWDDN;
+
+	/*
+	 * When exiting from idle state such as cpuidle or hotplug,
+	 * Enable PMU wait for the CPU to enter WFI when doing DFS
+	 * by setting CPUx Frequency ID to 1
+	 */
+	reg |= 1 << PMSU_CTL_CFG_CPU0_FRQ_ID_SFT;
 	writel(reg, pmsu_mp_base + PMSU_CTL_CFG(hw_cpu));
 
 	/* cancel Enable wakeup events and mask interrupts */
@@ -608,6 +615,38 @@ int armada_xp_pmsu_dfs_request(int cpu)
 	return 0;
 }
 
+void mvebu_v7_pmsu_disable_dfs_cpu(int hw_cpu)
+{
+	u32 reg;
+
+	if (pmsu_mp_base == NULL)
+		return;
+	/*
+	 * Disable PMU wait for the CPU to enter WFI when doing DFS
+	 * by setting CPUx Frequency ID to 0
+	 */
+	reg = readl(pmsu_mp_base + PMSU_CTL_CFG(hw_cpu));
+	reg &= ~(PMSU_CTL_CFG_CPU0_FRQ_ID_MSK << PMSU_CTL_CFG_CPU0_FRQ_ID_SFT);
+	writel(reg, pmsu_mp_base + PMSU_CTL_CFG(hw_cpu));
+}
+
+int armada_38x_pmsu_dfs_request(int cpu)
+{
+	/*
+	 * Protect CPU DFS from changing the number of online cpus number during
+	 * frequency transition by temporarily disable cpu hotplug
+	 */
+	cpu_hotplug_disable();
+
+	/* Trigger the DFS on all the CPUs */
+	on_each_cpu(mvebu_pmsu_dfs_request_local,
+		    NULL, false);
+
+	cpu_hotplug_enable();
+
+	return 0;
+}
+
 int mvebu_pmsu_dfs_request(int cpu)
 {
 	return mvebu_pmsu_dfs_request_ptr(cpu);
--- a/drivers/cpufreq/mvebu-cpufreq.c
+++ b/drivers/cpufreq/mvebu-cpufreq.c
@@ -30,7 +30,8 @@ static int __init mvebu_pmsu_cpufreq_ini
 	struct resource res;
 	int ret, cpu;
 
-	if (!of_machine_is_compatible("marvell,armadaxp"))
+	if (!of_machine_is_compatible("marvell,armadaxp") &&
+	    !of_machine_is_compatible("marvell,armada380"))
 		return 0;
 
 	/*
@@ -77,6 +78,8 @@ static int __init mvebu_pmsu_cpufreq_ini
 			return PTR_ERR(clk);
 		}
 
+		clk_prepare_enable(clk);
+
 		/*
 		 * In case of a failure of dev_pm_opp_add(), we don't
 		 * bother with cleaning up the registered OPP (there's
@@ -102,7 +105,14 @@ static int __init mvebu_pmsu_cpufreq_ini
 				__func__, ret);
 	}
 
-	mvebu_pmsu_dfs_request_ptr = armada_xp_pmsu_dfs_request;
+	if (of_machine_is_compatible("marvell,armada380")) {
+		if (num_online_cpus() == 1)
+			mvebu_v7_pmsu_disable_dfs_cpu(1);
+
+		mvebu_pmsu_dfs_request_ptr = armada_38x_pmsu_dfs_request;
+	} else if (of_machine_is_compatible("marvell,armadaxp")) {
+		mvebu_pmsu_dfs_request_ptr = armada_xp_pmsu_dfs_request;
+	}
 	platform_device_register_simple("cpufreq-dt", -1, NULL, 0);
 	return 0;
 }
--- a/include/linux/mvebu-pmsu.h
+++ b/include/linux/mvebu-pmsu.h
@@ -18,5 +18,7 @@ static inline int mvebu_pmsu_dfs_request
 #endif
 extern int (*mvebu_pmsu_dfs_request_ptr)(int cpu);
 int armada_xp_pmsu_dfs_request(int cpu);
+int armada_38x_pmsu_dfs_request(int cpu);
+void mvebu_v7_pmsu_disable_dfs_cpu(int hw_cpu);
 
 #endif /* __MVEBU_PMSU_H__ */
